#include "CMatrix4d.h"




CMatrix4d::CMatrix4d()
{
	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = 0; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix4d::CMatrix4d( const fixed* m )
{
	ASSERT( m ); 

	MEMCPY( this, m, sizeof( CMatrix4d ) );

	ASSERT( !m_orientationFixupIsApplicable || 
			( m_orientationFixupIsApplicable && m_nOfArthmeticOpsTriggeringOrientationFixup ) );
}

 
CMatrix4d::CMatrix4d( fixed m00, fixed m01, fixed m02, fixed m03,
					  fixed m10, fixed m11, fixed m12, fixed m13,
					  fixed m20, fixed m21, fixed m22, fixed m23,
					  fixed m30, fixed m31, fixed m32, fixed m33,
					  int16 nOfArthmeticOpsTriggeringOrientationFixup )
{ 
	m_col0.m_i = m00; 
	m_col0.m_j = m01;
	m_col0.m_k = m02;
	m_col0.m_w = m03;
	m_col1.m_i = m10; 
	m_col1.m_j = m11;
	m_col1.m_k = m12;
	m_col1.m_w = m13;
	m_col2.m_i = m20; 
	m_col2.m_j = m21;
	m_col2.m_k = m22;
	m_col2.m_w = m23;
	m_col3.m_i = m30; 
	m_col3.m_j = m31;
	m_col3.m_k = m32;
	m_col3.m_w = m33;

	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = nOfArthmeticOpsTriggeringOrientationFixup; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}



CMatrix4d::CMatrix4d( const CVector4d* col, int16 nOfArthmeticOpsTriggeringOrientationFixup )
{ 
	MEMCPY( this, col, sizeof( CVector4d ) * 4 );

	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = nOfArthmeticOpsTriggeringOrientationFixup; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix4d::CMatrix4d( const CVector4d& col0, const CVector4d& col1, 
					  const CVector4d& col2, const CVector4d& col3,
					  int16 nOfArthmeticOpsTriggeringOrientationFixup )
{ 
	m_col0 = col0; 
	m_col1 = col1; 
	m_col2 = col2; 
	m_col3 = col3; 

	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = nOfArthmeticOpsTriggeringOrientationFixup; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}



CMatrix4d& CMatrix4d::Transpose()
{
	fixed* a = (fixed*)this;
	fixed a1 = a[1];
	fixed a2 = a[2];
	fixed a3 = a[3];
	fixed a6 = a[6];
	fixed a7 = a[7];
	fixed a11 = a[11];

	a[1] = a[4]; a[2] = a[8]; a[3] = a[12];

	a[4] = a1; a[6] = a[9]; a[7] = a[13];

	a[8] = a2; a[9] = a6; a[11] = a[14];

	a[12] = a3; a[13] = a7; a[14] = a11;

	return *this;
}


CMatrix4d& CMatrix4d::operator*=( const CMatrix4d& b )
{
	fixed* a = (fixed*)this;
	fixed a0 = a[0];
	fixed a1 = a[1];
	fixed a2 = a[2];
	fixed a3 = a[3];
	fixed a4 = a[4];
	fixed a5 = a[5];
	fixed a6 = a[6];
	fixed a7 = a[7];
	fixed a8 = a[8];
	fixed a9 = a[9];
	fixed a10 = a[10];
	fixed a11 = a[11];
	fixed a12 = a[12];
	fixed a13 = a[13];
	fixed a14 = a[15];
	fixed a15 = a[15];

	a[0] = CMathFixed::Mul( a0, b[0] ) + CMathFixed::Mul( a4, b[1] ) + CMathFixed::Mul( a8, b[2] ) + CMathFixed::Mul( a12, b[3] );
	a[1] = CMathFixed::Mul( a1, b[0] ) + CMathFixed::Mul( a5, b[1] ) + CMathFixed::Mul( a9, b[2] ) + CMathFixed::Mul( a13, b[3] );
	a[2] = CMathFixed::Mul( a2, b[0] ) + CMathFixed::Mul( a6, b[1] ) + CMathFixed::Mul( a10, b[2] ) + CMathFixed::Mul( a14, b[3] );
	a[3] = CMathFixed::Mul( a3, b[0] ) + CMathFixed::Mul( a7, b[1] ) + CMathFixed::Mul( a11, b[2] ) + CMathFixed::Mul( a15, b[3] );

	a[4] = CMathFixed::Mul( a0, b[4] ) + CMathFixed::Mul( a4, b[5] ) + CMathFixed::Mul( a8, b[6] ) + CMathFixed::Mul( a12, b[7] );
	a[5] = CMathFixed::Mul( a1, b[4] ) + CMathFixed::Mul( a5, b[5] ) + CMathFixed::Mul( a9, b[6] ) + CMathFixed::Mul( a13, b[7] );
	a[6] = CMathFixed::Mul( a2, b[4] ) + CMathFixed::Mul( a6, b[5] ) + CMathFixed::Mul( a10, b[6] ) + CMathFixed::Mul( a14, b[7] );
	a[7] = CMathFixed::Mul( a3, b[4] ) + CMathFixed::Mul( a7, b[5] ) + CMathFixed::Mul( a11, b[6] ) + CMathFixed::Mul( a15, b[7] );

	a[8] = CMathFixed::Mul( a0, b[8] ) + CMathFixed::Mul( a4, b[9] ) + CMathFixed::Mul( a8, b[10] ) + CMathFixed::Mul( a12, b[11] );
	a[9] = CMathFixed::Mul( a1, b[8] ) + CMathFixed::Mul( a5, b[9] ) + CMathFixed::Mul( a9, b[10] ) + CMathFixed::Mul( a13, b[11] );
	a[10] = CMathFixed::Mul( a2, b[8] ) + CMathFixed::Mul( a6, b[9] ) + CMathFixed::Mul( a10, b[10] ) + CMathFixed::Mul( a14, b[11] );
	a[11] = CMathFixed::Mul( a3, b[8] ) + CMathFixed::Mul( a7, b[9] ) + CMathFixed::Mul( a11, b[10] ) + CMathFixed::Mul( a15, b[11] );

	a[12] += CMathFixed::Mul( a0, b[12] ) + CMathFixed::Mul( a4, b[13] ) + CMathFixed::Mul( a8, b[14] ) + CMathFixed::Mul( a12, b[15] );
	a[13] += CMathFixed::Mul( a1, b[12] ) + CMathFixed::Mul( a5, b[13] ) + CMathFixed::Mul( a9, b[14] ) + CMathFixed::Mul( a13, b[15] );
	a[14] += CMathFixed::Mul( a2, b[12] ) + CMathFixed::Mul( a6, b[13] ) + CMathFixed::Mul( a10, b[14] ) + CMathFixed::Mul( a14, b[15] );
	a[15] += CMathFixed::Mul( a3, b[12] ) + CMathFixed::Mul( a7, b[13] ) + CMathFixed::Mul( a11, b[14] ) + CMathFixed::Mul( a15, b[15] );

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}


CMatrix4d operator*( const CMatrix4d& a, const CMatrix4d& b )
{
	return CMatrix4d( CMathFixed::Mul( a[0], b[0] ) + CMathFixed::Mul( a[4], b[1] ) + CMathFixed::Mul( a[8], b[2] ) + CMathFixed::Mul( a[12], b[3] ),
					  CMathFixed::Mul( a[1], b[0] ) + CMathFixed::Mul( a[5], b[1] ) + CMathFixed::Mul( a[9], b[2] ) + CMathFixed::Mul( a[13], b[3] ),
					  CMathFixed::Mul( a[2], b[0] ) + CMathFixed::Mul( a[6], b[1] ) + CMathFixed::Mul( a[10], b[2] ) + CMathFixed::Mul( a[14], b[3] ),
					  CMathFixed::Mul( a[3], b[0] ) + CMathFixed::Mul( a[7], b[1] ) + CMathFixed::Mul( a[11], b[2] ) + CMathFixed::Mul( a[15], b[3] ),
	
					  CMathFixed::Mul( a[0], b[4] ) + CMathFixed::Mul( a[4], b[5] ) + CMathFixed::Mul( a[8], b[6] ) + CMathFixed::Mul( a[12], b[7] ),
					  CMathFixed::Mul( a[1], b[4] ) + CMathFixed::Mul( a[5], b[5] ) + CMathFixed::Mul( a[9], b[6] ) + CMathFixed::Mul( a[13], b[7] ),
					  CMathFixed::Mul( a[2], b[4] ) + CMathFixed::Mul( a[6], b[5] ) + CMathFixed::Mul( a[10], b[6] ) + CMathFixed::Mul( a[14], b[7] ),
					  CMathFixed::Mul( a[3], b[4] ) + CMathFixed::Mul( a[7], b[5] ) + CMathFixed::Mul( a[11], b[6] ) + CMathFixed::Mul( a[15], b[7] ),

					  CMathFixed::Mul( a[0], b[8] ) + CMathFixed::Mul( a[4], b[9] ) + CMathFixed::Mul( a[8], b[10] ) + CMathFixed::Mul( a[12], b[11] ),
					  CMathFixed::Mul( a[1], b[8] ) + CMathFixed::Mul( a[5], b[9] ) + CMathFixed::Mul( a[9], b[10] ) + CMathFixed::Mul( a[13], b[11] ),
					  CMathFixed::Mul( a[2], b[8] ) + CMathFixed::Mul( a[6], b[9] ) + CMathFixed::Mul( a[10], b[10] ) + CMathFixed::Mul( a[14], b[11] ),
					  CMathFixed::Mul( a[3], b[8] ) + CMathFixed::Mul( a[7], b[9] ) + CMathFixed::Mul( a[11], b[10] ) + CMathFixed::Mul( a[15], b[11] ),

					  CMathFixed::Mul( a[0], b[12] ) + CMathFixed::Mul( a[4], b[13] ) + CMathFixed::Mul( a[8], b[14] ) + CMathFixed::Mul( a[12], b[15] ),
					  CMathFixed::Mul( a[1], b[12] ) + CMathFixed::Mul( a[5], b[13] ) + CMathFixed::Mul( a[9], b[14] ) + CMathFixed::Mul( a[13], b[15] ),
					  CMathFixed::Mul( a[2], b[12] ) + CMathFixed::Mul( a[6], b[13] ) + CMathFixed::Mul( a[10], b[14] ) + CMathFixed::Mul( a[14], b[15] ),
					  CMathFixed::Mul( a[3], b[12] ) + CMathFixed::Mul( a[7], b[13] ) + CMathFixed::Mul( a[11], b[14] ) + CMathFixed::Mul( a[15], b[15] ),
					   
					  a.GetNumOfArithmeticOpsTriggeringOrientationFixup() );

}


inline
void CMatrix4d::IncrementOperationCount()
{
	if( m_nOfArithmeticOpsSinceLastFixup < MAX_UINT16 )
		m_nOfArithmeticOpsSinceLastFixup++;
}


inline
void CMatrix4d::ConsiderOrientationFixup()
{
	if( m_orientationFixupIsApplicable &&
		m_nOfArithmeticOpsSinceLastFixup >= m_nOfArthmeticOpsTriggeringOrientationFixup )
		CMatrix4d_ToCMatrix4dh( *this ).Orthonormalize();
}
